#include "obstacle_detector/bbox_processor.h"

namespace obstacle_detector
{
__global__ void get_inbbox_index(int16_t *points, uint8_t *point_bbox_index,
	int point_num,float *areas,int area_num,uint8_t *points_road_index,
	int *object_road_point_count,int roads_num)
{
	int threadid = blockDim.x * blockIdx.x + threadIdx.x;
	if (threadid>=(point_num*area_num))
	{
		return;
	}
	int area_id=threadid/point_num;
	int point_id=threadid%point_num;

	float x=points[point_id*4+0]*1.0/256;
	float y=points[point_id*4+1]*1.0/256;
	float bbox_yaw=areas[area_id*5+0];
	float center_x=areas[area_id*5+1];
	float center_y=areas[area_id*5+2];
	float length=areas[area_id*5+3]+0.5;
	float width=areas[area_id*5+4]+0.5;
	x-=center_x;
	y-=center_y;
	float bbox_x=x*cos(bbox_yaw)+y*sin(bbox_yaw);
	float bbox_y=-1*x*sin(bbox_yaw)+y*cos(bbox_yaw);
	if (bbox_x<-1*length/2 || bbox_x>length/2 || bbox_y<-1*width/2 || bbox_y>width/2)
	{
		return;
	}

	point_bbox_index[point_id]=area_id+1;
	atomicAdd(object_road_point_count+area_id*(roads_num+1)+0,1);
	int road_index=points_road_index[point_id]; //road_id已经加过1  1对应着第0个
	if (road_index>0)
	{
		atomicAdd(object_road_point_count+area_id*(roads_num+1)+road_index,1);
	}
	// uint16_t order=points_order[point_id*2+0];
	// uint16_t label=points_order[point_id*2+1];
	// int order_label=label+order*65535;
	// inbbox_order[order_label]=area_id+1;
	// if (area_id==0)
	// {
	// 	printf("in bbox1: %d,%d\n",point_id,order_label);
	// }
	// if (order_label==101341)
	// {
	// 	printf ("check inbbox1: %d,%d,%d\n",point_id,area_id,
	// 		inbbox_order[order_label]);
	// }
}

__global__ void get_occupancy_grid(int16_t *points, uint8_t *point_bbox_index,
	int point_num,uint8_t *occupancy_grid,float min_x,float max_x,float min_y,
	float max_y,float resolution,int grid_x_size,int grid_y_size)
{
	int threadid = blockDim.x * blockIdx.x + threadIdx.x;
	if (threadid>=point_num)
	{
		return;
	}

	// uint16_t order=points_order[threadid*2+0];
	// uint16_t label=points_order[threadid*2+1];
	// int order_label=label+order*65535;
	uint8_t bbox_label=point_bbox_index[threadid];
	if (bbox_label==0)
	{
		return;
	}

	float x=points[threadid*4+0]*1.0/256;
	float y=points[threadid*4+1]*1.0/256;
	if (x<min_x || x>max_x || y<min_y || y>max_y)
	{
		return;
	}
	int x_index=(x-min_x)/resolution;
	int y_index=(y-min_y)/resolution;
	occupancy_grid[x_index*grid_y_size+y_index]=bbox_label;
}

__global__ void velocity_score_check(int16_t *points, uint8_t *point_bbox_index,
	int point_num,float *velocity,int velocity_num,int *velocity_score,
	uint8_t *occupancy_grid, uint8_t *objects_related,uint8_t *lst_related,
	float min_x,float max_x,float min_y,float max_y,float resolution,int grid_x_size,
	int grid_y_size,float loc_x,float loc_y,float loc_yaw,float lst_loc_x,
	float lst_loc_y,float lst_loc_yaw)
{
	int threadid = blockDim.x * blockIdx.x + threadIdx.x;
	if (threadid>=(point_num*velocity_num))
	{
		return;
	}

	int point_id=threadid%point_num;
	int velocity_id=threadid/point_num;

	// uint16_t order=points_order[point_id*2+0];
	// uint16_t label=points_order[point_id*2+1];
	// int order_label=label+order*65535;
	uint8_t bbox_label=point_bbox_index[point_id];
	// if (order_label==101341 && velocity_id==0)
	// {
	// 	printf ("check inbbox 2: %d,%d\n",point_id,bbox_label);
	// }
	if (bbox_label==0) return;
	bbox_label-=1;

	if (velocity_id==0)
	{
		// if (bbox_label==0)
		// {
		// 	printf("in bbox2: %d,%d\n",point_id,order_label);
		// }
		atomicAdd(velocity_score+(velocity_num+1)*bbox_label+velocity_num,1);
	}

	float x=points[point_id*4+0]*1.0/256;
	float y=points[point_id*4+1]*1.0/256;
	float new_x=x*cos(-1*loc_yaw)+y*sin(-1*loc_yaw)+loc_x;
	float new_y=-1*x*sin(-1*loc_yaw)+y*cos(-1*loc_yaw)+loc_y;
	float x_v=new_x-velocity[(bbox_label)*(velocity_num*2)+velocity_id*2+0];
	float y_v=new_y-velocity[(bbox_label)*(velocity_num*2)+velocity_id*2+1];
	new_x=x_v-lst_loc_x;
	new_y=y_v-lst_loc_y;
	float lst_x=new_x*cos(lst_loc_yaw)+new_y*sin(lst_loc_yaw);
	float lst_y=-1*new_x*sin(lst_loc_yaw)+new_y*cos(lst_loc_yaw);
	if (lst_x<min_x || lst_x>max_x || lst_y<min_y || lst_y>max_y)
	{
		return;
	}
	int x_index=(lst_x-min_x)/resolution;
	int y_index=(lst_y-min_y)/resolution;
	uint8_t lst_label=occupancy_grid[x_index*grid_y_size+y_index];
	// if (bbox_label==2 && velocity_id==0)
	// {
	// 	printf ("lst xy: %f,%f,%d,%d,%d\n",lst_x,lst_y,x_index,y_index,lst_label);	
	// }
	if (lst_label==0) return;
	lst_label-=1;
	if (objects_related[bbox_label]==lst_related[lst_label])
	{
		atomicAdd(velocity_score+(velocity_num+1)*bbox_label+velocity_id,1);
	}
}

BBoxPorcessor::BBoxPorcessor(bool debug):
debug_(debug)
{
	grid_dim_[0]=(check_range[1]-check_range[0])/resolution_;
	grid_dim_[1]=(check_range[3]-check_range[2])/resolution_;
	cudaMalloc((void **)&region_data_gpu_,sizeof(float)*10000*5);
	// cudaMalloc((void **)&inbbox_order_gpu_,sizeof(uint8_t)*200000);
	cudaMalloc((void **)&point_bbox_index_gpu_,sizeof(uint8_t)*200000);
	cudaMalloc((void **)&object_road_point_count_gpu_,sizeof(int)*1000*100);
	cudaMalloc((void **)&occupancy_grid_gpu_,sizeof(uint8_t)*grid_dim_[0]*grid_dim_[1]);
	cudaMalloc((void **)&lst_related_gpu_,sizeof(uint8_t)*1000);
	cudaMalloc((void **)&objects_related_gpu_,sizeof(uint8_t)*1000);
	cudaMalloc((void **)&objects_velocity_gpu_,sizeof(float)*1000*14);
	cudaMalloc((void **)&velocity_score_gpu_,sizeof(int)*1000*8);

	convex_hull_=std::make_unique<ConvexHull>(debug);

	// cudaMalloc((void **)&valid_points_gpu_,sizeof(int16_t)*200000*4);
	// cudaMalloc((void **)&valid_points_order_gpu_,sizeof(uint16_t)*200000*2);
	// cudaMalloc((void **)&valid_points_num_gpu_,sizeof(int));

	if (debug_)
	{
		debug_display_=std::make_unique<DebugDisplay>("bbox_processor");
	}
}

void BBoxPorcessor::bbox_process(const module_base::Location &loc,
	int16_t *points,int &point_num,std::vector<Object> &all_objects,
	std::vector<Object> &lst_objects,uint8_t *point_road_index, 
	int roads_num)
{
	bbox_nms(all_objects);

	int area_num=all_objects.size();
	float *area_data=new float [area_num*5];
	for (int i=0;i<area_num;i++)
	{
		area_data[i*5+0]=all_objects[i].yaw;
		area_data[i*5+1]=all_objects[i].center(0);
		area_data[i*5+2]=all_objects[i].center(1);
		area_data[i*5+3]=all_objects[i].dim(0);
		area_data[i*5+4]=all_objects[i].dim(1);
	}
	cudaMemcpy(region_data_gpu_,area_data,sizeof(float)*area_num*5,
		cudaMemcpyHostToDevice);
	cuda_base::CUDA_CHECK_RETURN(cudaGetLastError());
	delete [] area_data;

	// printf ("area_num: %d\n",area_num);
	if (area_num==0)
	{
		return;
	}
	cuda_base::CUDA_CHECK_RETURN(cudaGetLastError());
	cudaMemset(point_bbox_index_gpu_,0,sizeof(uint8_t)*200000);
	cuda_base::CUDA_CHECK_RETURN(cudaGetLastError());
	cudaMemset(object_road_point_count_gpu_,0,sizeof(int)*area_num*(roads_num+1));
	printf("pts num , area_num, road num %d %d %d \n",point_num,area_num,roads_num);
	cuda_base::CUDA_CHECK_RETURN(cudaGetLastError());

	int grid_num1=(point_num*area_num+1024-1)/1024;
	get_inbbox_index<<<grid_num1,1024>>>(points,point_bbox_index_gpu_,
		point_num,region_data_gpu_,area_num,point_road_index,
		object_road_point_count_gpu_,roads_num);
	cuda_base::CUDA_CHECK_RETURN(cudaGetLastError());
	std::vector<Eigen::MatrixXf> objects_min_bbox;
	convex_hull_->get_convex_hull(points,point_num,point_bbox_index_gpu_,
		area_num+1,loc,objects_min_bbox);
	cuda_base::CUDA_CHECK_RETURN(cudaGetLastError());
	int *object_road_point_count=new int [area_num*(roads_num+1)];
	cudaMemcpy(object_road_point_count,object_road_point_count_gpu_,
		sizeof(int)*area_num*(roads_num+1),cudaMemcpyDeviceToHost);
	cuda_base::CUDA_CHECK_RETURN(cudaGetLastError());
	for (int i=0;i<all_objects.size();i++)
	{
		int total_num=object_road_point_count[i*(roads_num+1)];
		// printf ("objects %d(%d) points %d: ",i,all_objects[i].label,
		// 	total_num);
		for (int j=0;j<roads_num;j++)
		{
			int num=object_road_point_count[i*(roads_num+1)+j+1];
			if (num*1.0/total_num>0.9)
			{
				all_objects[i].road_id=j;
			}
			// printf ("%d ",object_road_point_count[i*(roads_num+1)+j+1]);
		}
		// printf ("\n");
		all_objects[i].min_bbox=objects_min_bbox[i];
		// all_objects[i].point_num=total_num;
	}
	delete [] object_road_point_count;
	cuda_base::CUDA_CHECK_RETURN(cudaGetLastError());
	bbox_compare(all_objects,lst_objects);
	cuda_base::CUDA_CHECK_RETURN(cudaGetLastError());


	cudaMemset(velocity_score_gpu_,0,sizeof(int)*all_objects.size()*(option_v_num_+1));
	//计算每个速度对应的score
	int grid_num2=(point_num*option_v_num_+1024-1)/1024;
	cuda_base::CUDA_CHECK_RETURN(cudaGetLastError());
	velocity_score_check<<<grid_num2,1024>>>(points, point_bbox_index_gpu_,
		point_num,objects_velocity_gpu_,option_v_num_,velocity_score_gpu_,
		occupancy_grid_gpu_, objects_related_gpu_,lst_related_gpu_,
		check_range[0],check_range[1],check_range[2],check_range[3],resolution_,
		grid_dim_[0],grid_dim_[1],loc.x,loc.y,loc.yaw,lst_loc_.x,
		lst_loc_.y,lst_loc_.yaw);
	cuda_base::CUDA_CHECK_RETURN(cudaGetLastError());
	int *velocity_score=new int [all_objects.size()*(option_v_num_+1)];
	cudaMemcpy(velocity_score,velocity_score_gpu_,sizeof(int)*(option_v_num_+1)*all_objects.size(),
		cudaMemcpyDeviceToHost);
	cuda_base::CUDA_CHECK_RETURN(cudaGetLastError());
	for (int i=0;i<all_objects.size();i++)
	{
		// printf ("center %d: %f,%f,%f,%f\n",i,all_objects[i].center(0),all_objects[i].center(1),
		// 	all_objects[i].global_center(0),all_objects[i].global_center(1));
		// printf ("score %d(%d): %d,%d,%d,%d,%d,%d,%d,%d\n",i,all_objects[i].label,velocity_score[i*8+0],
		// 	velocity_score[i*8+1],velocity_score[i*8+2],velocity_score[i*8+3],
		// 	velocity_score[i*8+4],velocity_score[i*8+5],velocity_score[i*8+6],
		// 	velocity_score[i*8+7]);
		auto max_score=std::max_element(velocity_score+i*(option_v_num_+1), velocity_score+i*(option_v_num_+1)+option_v_num_);
		int max_index=std::distance(velocity_score+i*(option_v_num_+1),max_score);
		// if (all_objects[i].label==3)
		// {
		// 	printf ("max_index: %d,%d\n",(*max_score),max_index);
		// }
		Eigen::Vector2f velocity;
		velocity<<0,0;
		// if ((*max_score)<10) continue;
		if ((*max_score)>(velocity_score[i*(option_v_num_+1)+option_v_num_]*0.2))
		{
			velocity(0)=objects_velocity_[i*(option_v_num_*2)+max_index*2+0];
			velocity(1)=objects_velocity_[i*(option_v_num_*2)+max_index*2+1];
		}
		//锁站和灯塔速度为0
		if (all_objects[i].label==4 || all_objects[i].label==6)
		{
			velocity<<0,0;
		}
		// if (all_objects[i].global_center(0)>-35 && all_objects[i].global_center(0)<-25 &&
		// 	all_objects[i].global_center(1)>170 && all_objects[i].global_center(1)<180 &&
		// 	all_objects[i].label==3)
		// {
		// if (all_objects[i].label==1 && all_objects[i].center(1)<0)
		// {
		// // 	printf ("id: %s\n",all_objects[i].id.c_str());
		// 	printf ("center %d: %f,%f,%f,%f\n",i,all_objects[i].center(0),all_objects[i].center(1),
		// 		all_objects[i].global_center(0),all_objects[i].global_center(1));
		// 	for (int j=0;j<option_v_num_;j++)
		// 	{
		// 		printf ("optional velocity %d: %f,%f\n",i,objects_velocity_[i*(option_v_num_*2)+j*2+0],
		// 			objects_velocity_[i*(option_v_num_*2)+j*2+1]);
		// 	}
		// 	for (int k=0;k<all_objects[i].history_v.size();k++)
		// 	{
		// 		printf ("history velocity %d: %f,%f\n",k,all_objects[i].history_v[k](0),
		// 			all_objects[i].history_v[k](1));
		// 	}
		// 	printf ("score %d(%d): ",i,all_objects[i].label);
		// 	for (int j=0;j<option_v_num_+1;j++)
		// 	{
		// 		printf ("%d ",velocity_score[i*(option_v_num_+1)+j]);
		// 	}
		// 	printf ("\n");
		// 	printf ("max_index: %d,%d\n",(*max_score),max_index);
		// 	printf ("velocity: %f,%f,%d\n",velocity(0),velocity(1),all_objects[i].history_v.size());
		// }
		// printf ("velocity %d: %d,%d,%f,%f\n",i,(*max_score),max_index,
		// 	velocity[0],velocity[1]);
		all_objects[i].velocity=velocity;
		all_objects[i].add_v2history(velocity);
		// all_objects[i].velocity(0)=v_x;
		// all_objects[i].velocity(1)=v_y;

	}
	delete [] velocity_score;

	cuda_base::CUDA_CHECK_RETURN(cudaGetLastError());
	cudaMemset(occupancy_grid_gpu_,0,sizeof(uint8_t)*grid_dim_[0]*grid_dim_[1]);
	int grid_num3=(point_num+1024-1)/1024;
	get_occupancy_grid<<<grid_num3,1024>>>(points, point_bbox_index_gpu_,
		point_num,occupancy_grid_gpu_,check_range[0],check_range[1],check_range[2],
		check_range[3],resolution_,grid_dim_[0],grid_dim_[1]);
	cuda_base::CUDA_CHECK_RETURN(cudaGetLastError());
	lst_loc_=loc;

	if (debug_)
	{
		int16_t *points_debug=new int16_t [point_num*4];
		cudaMemcpy(points_debug,points,sizeof(int16_t)*point_num*4,
			cudaMemcpyDeviceToHost);
		float *points_float=new float [point_num*4];
		for (int i=0;i<point_num;i++)
		{
			float x=points_debug[i*4+0]*1.0/256;
			float y=points_debug[i*4+1]*1.0/256;
			float new_x=x*cos(-1*loc.yaw)+y*sin(-1*loc.yaw);
			float new_y=-1*x*sin(-1*loc.yaw)+y*cos(-1*loc.yaw);
			points_float[i*4+0]=new_x+loc.x;
			points_float[i*4+1]=new_y+loc.y;
			points_float[i*4+2]=points_debug[i*4+2]*1.0/256;

		}
		debug_display_->set_showinfo_float(points_float,point_num);
		delete [] points_debug;
		delete [] points_float;
	}


	// cudaMemset(valid_points_num_gpu_,0,sizeof(int));
	// int grid_num2=(point_num+1024-1)/1024;
	// get_valid_points<<<grid_num2,1024>>>(points,points_order,
	// 	point_num,eliminated_order_gpu_,valid_points_gpu_,
	// 	valid_points_order_gpu_,valid_points_num_gpu_);
	// int final_points_num;
	// cudaMemcpy(&final_points_num,valid_points_num_gpu_,sizeof(int),
	// 	cudaMemcpyDeviceToHost);
	// printf ("final_points_num: %d\n",final_points_num);

	// point_num=final_points_num;
	// cudaMemcpy(points,valid_points_gpu_,sizeof(int16_t)*point_num*4,
	// 	cudaMemcpyDeviceToDevice);
	// cudaMemcpy(points_order,valid_points_order_gpu_,sizeof(uint16_t)*point_num*2,
	// 	cudaMemcpyDeviceToDevice);

	// int16_t *debug_points=new int16_t [point_num*4];
	// cudaMemcpy(debug_points,points,sizeof(int16_t)*point_num*4,
	// 	cudaMemcpyDeviceToHost);
	// uint16_t *debug_points_order=new uint16_t[point_num*2];
	// cudaMemcpy(debug_points_order,points_order,sizeof(uint16_t)*point_num*2,
	// 	cudaMemcpyDeviceToHost);
	// bool eliminated_order[200000];
	// cudaMemcpy(eliminated_order,eliminated_order_gpu_,sizeof(bool)*200000,
	// 	cudaMemcpyDeviceToHost);
	// for (int i=0;i<point_num;i++)
	// {
	// 	float x=debug_points[i*4+0]*1.0/256;
	// 	float y=debug_points[i*4+1]*1.0/256;
	// 	for (int j=10;j<11;j++)
	// 	{
	// 		float bbox_yaw=area_data[j*5+0];
	// 		float center_x=area_data[j*5+1];
	// 		float center_y=area_data[j*5+2];
	// 		float length=area_data[j*5+3];
	// 		float width=area_data[j*5+4];
	// 		float new_x=x-center_x;
	// 		float new_y=y-center_y;
	// 		float bbox_x=new_x*cos(bbox_yaw)+new_y*sin(bbox_yaw);
	// 		float bbox_y=-1*new_x*sin(bbox_yaw)+new_y*cos(bbox_yaw);
	// 		if (bbox_x<-1*length/2 || bbox_x>length/2 || bbox_y<-1*width/2 || bbox_y>width/2)
	// 		{
	// 			continue;
	// 		}
	// 		uint16_t order=debug_points_order[i*2+0];
	// 		uint16_t label=debug_points_order[i*2+1];
	// 		int order_label=label+order*65535;
	// 		printf ("%d point in area %d: %d,%d,%d,%d,%f,%f,%f,%f,%f,%f\n",i,j,order,
	// 			label,order_label,eliminated_order[order_label],bbox_x,bbox_y,length,width,new_x,new_y);
	// 		break;
	// 	}
	// }

	// delete [] debug_points;


	cuda_base::CUDA_CHECK_RETURN(cudaGetLastError());


}

void BBoxPorcessor::bbox_compare(std::vector<Object> &all_objects,
	std::vector<Object> &lst_objects)
{
	if (debug_)
	{
		debug_display_->show3Dbbox_compare(all_objects,lst_objects);
	}

	//上一帧中两个物体是否关联
	uint8_t *lst_related=new uint8_t [lst_objects.size()];
	for (int i=0;i<lst_objects.size();i++)
	{
		lst_related[i]=i;
	}

	//这一帧的障碍物关联到上一帧的障碍物
	uint8_t *objects_related=new uint8_t [lst_objects.size()+all_objects.size()];
	for (int i=0;i<(lst_objects.size()+all_objects.size());i++)
	{
		objects_related[i]=-1;
	}

	//这一帧障碍物的备选速度 每个障碍物生成7个速度，4个角点 中心点 上一帧速度 0速
	// float *objects_velocity=new float [(lst_objects.size()+all_objects.size())*7*2];


	std::vector<std::vector<int>> lst_match_count(lst_objects.size());
	std::vector<std::vector<int>> match_count(all_objects.size());
	for (int i=0;i<all_objects.size();i++)
	{
		int label1=all_objects[i].label;
		// if (label1!=2) continue;
		int count=0;
		for (int j=0;j<lst_objects.size();j++)
		{
			int label2=lst_objects[j].label;
			if (label1!=label2)
			{
				continue;
			}

			float iou=bbox_iou(all_objects[i],lst_objects[j]);
			// printf ("iou %d,%d: %d,%f\n",i,j,label1,iou);
			if (iou>0)
			{
				// printf ("iou %d,%d: %d,%f\n",i,j,label1,iou);
				lst_match_count[j].push_back(i);
				match_count[i].push_back(j);
			}
		}

	}

	for (int i=0;i<match_count.size();i++)
	{
		if (match_count[i].size()==0)
		{
			// printf ("objet %d(%d) no match in lst frame\n",i,all_objects[i].label);
			// all_objects[i].add_loc2history();
		}
		else if (match_count[i].size()==1)
		{
			all_objects[i].lost_count=0;
			int match_id=match_count[i][0];
			objects_related[i]=match_id;
			if (lst_match_count[match_id].size()==1)
			{
				all_objects[i].id=lst_objects[match_id].id;
				all_objects[i].check_count=lst_objects[match_id].check_count+1;
				all_objects[i].history_v=lst_objects[match_id].history_v;
				// all_objects[i].add_loc2history();
			}
			else if (lst_match_count[match_count[i][0]].size()>1) //上一帧的框对应到多个这一帧的框
			{
				//变成多个新的物体 速度为0
				// printf ("objet %d(%d) match more than 1 object in current frame\n",i,all_objects[i].label);
				all_objects[i].history_v=lst_objects[match_id].history_v;

			}

		}
		else if (match_count[i].size()>=1) //这一帧的框对应到多个上一帧的框
		{
			//上一帧的点的label需要合并
			// printf ("objet %d(%d) match more than 1 object in lst frame\n",i,all_objects[i].label);
			all_objects[i].lost_count=0;
			int match_id=match_count[i][0];
			objects_related[i]=match_id;
			all_objects[i].id=lst_objects[match_id].id;
			all_objects[i].check_count=lst_objects[match_id].check_count+1;
			all_objects[i].history_v=lst_objects[match_id].history_v;
		}
	}


	for (int i=0;i<lst_match_count.size();i++)
	{
		if (lst_match_count[i].size()==0)
		{
			lst_objects[i].lost_count+=1;
			// printf ("objet %d(%d) lost in current frame: %d\n",i,lst_objects[i].label,lst_objects[i].lost_count);
			if (lst_objects[i].lost_count<=3)
			{
				objects_related[all_objects.size()]=i;
				all_objects.push_back(lst_objects[i]);
				match_count.push_back(std::vector<int>(1,i));
			}
		}
	}

	// printf ("all_objects size before velocity: %d\n",all_objects.size());

	get_optional_velocity(all_objects,lst_objects,match_count,objects_velocity_);
	// printf ("get get_optional_velocity\n");
	// for (int i=0;i<all_objects.size();i++)
	// {
	// 	printf ("object related %d: %d\n",i,objects_related[i]);
	// }
	// for (int i=0;i<lst_objects.size();i++)
	// {
	// 	printf ("lst_object related %d: %d\n",i,lst_related[i]);
	// }
	cudaMemcpy(lst_related_gpu_,lst_related,sizeof(uint8_t)*lst_objects.size(),
		cudaMemcpyHostToDevice);
	cudaMemcpy(objects_related_gpu_,objects_related,sizeof(uint8_t)*all_objects.size(),
		cudaMemcpyHostToDevice);
	cudaMemcpy(objects_velocity_gpu_,objects_velocity_,sizeof(float)*all_objects.size()*14,
		cudaMemcpyHostToDevice);

	// delete [] objects_velocity;
	delete [] lst_related;
	delete [] objects_related;

}

void BBoxPorcessor::get_optional_velocity(const std::vector<Object> &all_objects,
	const std::vector<Object> &lst_objects,
	const std::vector<std::vector<int>> &matches,
	float *objects_velocity)
{
	for (int i=0;i<all_objects.size();i++)
	{
		if (matches[i].size()==0 || matches[i].size()>1)
		{
			for (int j=0;j<option_v_num_*2;j++)
			{
				objects_velocity[i*(option_v_num_*2)+j]=0;
			}
		}
		else if (matches[i].size()==1)
		{
			// printf ("get objects_velocity %d:  ",i);
			Eigen::MatrixXf bbox1=all_objects[i].min_bbox;
			Eigen::MatrixXf bbox2=lst_objects[matches[i][0]].min_bbox;
			Eigen::MatrixXf v1;
			get_bbox_velocity(all_objects[i].label,bbox1,bbox2,v1);

			// printf ("v2: %f,%f\n",v2(0),v2(1));
			for (int j=0;j<v1.cols();j++)
			{
				objects_velocity[i*(option_v_num_*2)+j*2+0]=v1(0,j);
				objects_velocity[i*(option_v_num_*2)+j*2+1]=v1(1,j);
			}
			objects_velocity[i*(option_v_num_*2)+5*2+0]=0;
			objects_velocity[i*(option_v_num_*2)+5*2+1]=0;
			objects_velocity[i*(option_v_num_*2)+6*2+0]=lst_objects[matches[i][0]].velocity(0);
			objects_velocity[i*(option_v_num_*2)+6*2+1]=lst_objects[matches[i][0]].velocity(1);

			// Eigen::MatrixXf bbox3=all_objects[i].global_bbox;
			// Eigen::MatrixXf bbox4=lst_objects[matches[i][0]].global_bbox;
			// Eigen::MatrixXf v3=bbox3-bbox4;
			// Eigen::Vector2f v4=bbox3.rowwise().mean()-bbox4.rowwise().mean();
			// for (int j=0;j<v3.cols();j++)
			// {
			// 	objects_velocity[i*(option_v_num_*2)+(j+7)*2+0]=v3(0,j);
			// 	objects_velocity[i*(option_v_num_*2)+(j+7)*2+1]=v3(1,j);
			// }
			// objects_velocity[i*(option_v_num_*2)+11*2+0]=v4(0);
			// objects_velocity[i*(option_v_num_*2)+11*2+1]=v4(1);
		}

		// for (int j=0;j<7;j++)
		// {
		// 	printf ("velocity %d: %d,%f,%f\n",i,j,objects_velocity[i*14+j*2+0],
		// 		objects_velocity[i*14+j*2+1]);
		// }
	}
}


}